#include "bvh.h"
#include "utils.h"

#include <algorithm>

namespace tls{

  bvh::bvh(const linear_aggr& aggr):mesh(aggr){
    n_nodes=2*mesh.faces.size() -1;
    nodes= new node[n_nodes];

    build(nodes,0,mesh.faces.size());
  }
  bvh::~bvh(){
    delete[] nodes;
  }

  bvh::node* bvh::build(bvh::node* root,int start,int end){
    if(end==start+1){
      root->start=start;
      root->end=end;
      root->axis=-1;
      root->box=mesh.faces[start].tri.box;
      root->right=0;
      return root;
    }

    aabb extent,center;

    for(int i=start;i<end;++i){
      extent=extent.make_union(mesh.faces[i].tri.box);
      center=center.make_union(mesh.faces[i].tri.box.center());
    }

    int axis=0;
    vec3 d=center.u - center.l;
    if(d[1] > d[axis]) axis=1;
    if(d[2] > d[axis]) axis=2;


    if(equ(d.x,0) && equ(d.y,0) && equ(d.z,0)){
      root->start=start;
      root->end=end;
      root->axis=-1;
      root->box=extent;
      root->right=0;
      return root;
    }
    

    real mid=center.center()[axis];

    vector<triangle>::iterator p=
      std::partition(mesh.faces.begin()+start,mesh.faces.begin()+end,
		     comp(axis,mid));

    root->start=start;
    root->end=end;
    root->axis=axis;
    root->box=extent;

    int m=p - mesh.faces.begin();
    node* t=build(root+1,start,m);
    root->right=t+1;
    return build(t+1,m,end);
  }

  bool bvh::intersect(ray& r,isect& hit)const{
    node* todo[64];
    int top=0;
    bool bhit=false;
    node* iter=nodes;

    while(true){
      ray temp(r);
      if(iter->box.in(temp.o) || iter->box.intersect(temp)){
	//an external node
	if(iter->axis==-1){
	  for(int j=iter->start;j<iter->end;++j)
	    if(mesh.faces[j].intersect(r,hit))
	      bhit=true;

	  if(top==0) break;
	  iter=todo[--top];
	}
	else{
	  todo[top++]=iter->right;
	  iter++;
	}
      }
      else{
	if(top==0) break;
	iter=todo[--top];
      }
    }//while true
    return bhit;
  }


  bool bvh::intersectP(ray& r)const{
    node* todo[64];
    int top=0;
    node* iter=nodes;
    isect hit;

    while(true){
      ray temp(r);
      if(iter->box.in(temp.o) || iter->box.intersect(temp)){
	//an external node
	if(iter->axis==-1){
	  for(int j=iter->start;j<iter->end;++j)
	    if(mesh.faces[j].intersect(r,hit))
	      return true;

	  if(top==0) break;
	  iter=todo[--top];
	}
	else{
	  todo[top++]=iter->right;
	  iter++;
	}
      }
      else{
	if(top==0) break;
	iter=todo[--top];
      }
    }//while true
    return false;
  }

}//namespace tls

